#include "WiFly.h"
#include "Servo.h"
#include "parsing.h"
#include "functions.h"

#define MAXVALUES 20

#define ZERO_AV 108
#define ZERO_AR 97


char passphrase[] = "WifiCampus";
char ssid[] = "WIFI-ECOLE-WPA";

Server server(80);

Servo servoAvant;
Servo servoArriere;
Servo motorProp;


double posServoAvant;
double posServoArriere;


void setup() {
  
  Serial.begin(9600);
  Serial.println("Setting up");
  WiFly.begin();
  Serial.println("WiFly started");
   
  
if (!WiFly.join(ssid, passphrase)) {
    while (1) {
      
      Serial.print("Error");
    }
  }

  Serial.print("IP: ");
  Serial.println(WiFly.ip());
  servoAvant.attach(5);
  servoArriere.attach(6);
  motorProp.attach(3);
  
  initMotors();

  server.begin();
}


 void initArray(char array[])
 {
   int i = 0 ;
   for (i ; i < MAXVALUES ; i++)
   {
      array[i] = '\0'; 
   }
 }
 
 void cloneArray(char a[], char b[])
 {
   int i = 0;
   
   for (i ; i < MAXVALUES ; i++)
   {
      b[i] = a[i]; 
   }
   
   initArray(a);
   
 }

void loop() {

  Client client = server.available();


  char buffer[MAXVALUES];
  char commandes[MAXVALUES];
  initArray(buffer);
  initArray(commandes);
  
  int i = 0;
  
  if (client) 
  {

    boolean current_line_is_blank = true;
    while (client.connected()) 
    {
       if(client.available())
       {        
        Serial.println("Je suis ici"); 
        client.write("Coucou\n");
        
        char c = client.read();
        if (c=='!')
        {
          buffer[i] = c;
          cloneArray(buffer,commandes);
          i=0;
          actions(commandes);   
        }
        
        if((c != '\n') && (c != ';') && (c != '!'))
        {
          buffer[i] = c; 
          i++;
        }            
       }
    }
    delay(100);
    client.stop();
  }
}



void actions(char comm[])
{
  commande com;
  init_com(com);
  
  int parameter;
  
  parseFunction(comm, com) ;
  parameter = atoi(parseParameter(comm, com));
  
  
  Serial.println(com.parsedFunction);
  Serial.println(com.parsedParameter);
  
  
  
    
  if(strcmp(com.parsedFunction,"init") == 0)
  {
    initMotors();
  }
  
  if(strcmp(com.parsedFunction,"turnAV") == 0)
  {
    turn(parameter,0);
  }
  
  if(strcmp(com.parsedFunction,"turnAR") == 0)
  {
    turn(parameter,1);
  }
  
  if(strcmp(com.parsedFunction,"turnX") == 0)
  {
    turn(parameter,2);
  }
  
  if(strcmp(com.parsedFunction,"turnCR") == 0)
  {
    turn(parameter,3);
  }
  
  
  if(strcmp(com.parsedFunction,"off") == 0)
  {
    off();
  }
  
  if(strcmp(com.parsedFunction,"seta") == 0)
  {
    setAngle(parameter,0);
  }
  
  if(strcmp(com.parsedFunction,"PWM") == 0)
  {
    setPWM(parameter);
  }
  
}


 void initMotors()
 {
   posServoAvant = ZERO_AV ;
   posServoArriere = ZERO_AR ;
   
   servoAvant.write(posServoAvant);
   servoArriere.write(posServoArriere);
 }

// Tourner à droite : essieu avant vers la droite + essieu arrière vers la gauche

void setAngle(int angle, int motor)
{
  switch(motor)
  {
    case 0:
      posServoAvant = ZERO_AV - angle;
      servoAvant.write(posServoAvant);
    break;
    
    case 1:
      posServoArriere = ZERO_AR - angle;
      servoArriere.write(posServoArriere);
    break;
    
    case 2:
      posServoAvant = ZERO_AV - angle;
      posServoArriere = ZERO_AR - angle;
      servoAvant.write(posServoAvant);
      servoArriere.write(posServoArriere);
    break;
    
    case 3:
      posServoAvant = ZERO_AV - angle;
      posServoArriere = ZERO_AR + angle;
      servoAvant.write(posServoAvant);
      servoArriere.write(posServoArriere);
    break;
    
  }
}

void turn(int angle, int motor) // On tourne dans l'absolu.
{
  switch(motor)
  {
    case 0:
      setAngle(angle,motor);
    break;
    
    case 1:
      setAngle(angle,motor);
    break;
    
    case 2:
      setAngle(angle,motor);
      setAngle(angle,motor);
    break ;
    
    case 3:
      setAngle(angle,motor);
      setAngle(angle,motor);
    break ;

  }
  
}

void off()
{
  motorProp.write(92);
}


void setPWM(int pwm)
{
      motorProp.write(pwm);
}




